//
//  DuoAccessoryV2.m
//  duosample
//
//  For devices with analog control sticks.
//
//  Created by Lin Xu for Discovery Bay Games on 4/4/12.
//  Copyright (c) 2012 Discovery Bay Games. All rights reserved.
//

#include "iConfig.hpp"

#if GLF_COMPILE_WITH_DUO_CONTROLLER_SUPPORT

#import "input/duo/DuoAccessoryV2.h"
#import "input/duo/DuoAccessoryDevice.h"

@interface DuoAccessoryV2 () /*{    
    DuoInputPackV2 _inputPacket;
    id<DuoAccessoryDelegateV2> _delegate;
    NSMutableData *rxData;
}*/

- (int) readData:(NSData *) data;
@end

@implementation DuoAccessoryV2
@synthesize delegate = _delegate;
//@synthesize inputPacket = _inputPacket;


- (void) initializePacket
{
    memset(&_inputPacket, 0, sizeof(_inputPacket));
}

- (int) readData:(NSData *) data
{
    const int  MASK_BUTT_A = 0x01;
    const int  MASK_BUTT_B = 0x02;
    const int  MASK_BUTT_X = 0x04;
    const int  MASK_BUTT_Y = 0x08;
    
    const int  MASK_BUTT_START =   0x10;
    const int  MASK_BUTT_L_STICK = 0x20;
    const int  MASK_BUTT_R_STICK = 0x40;
    
    const int  MASK_TRIG_R = 0x01;
    const int  MASK_TRIG_L = 0x02;
    const int  MASK_BUTT_U = 0x04;
    const int  MASK_BUTT_R = 0x08;
    const int  MASK_BUTT_D = 0x10;
    const int  MASK_BUTT_L = 0x20;

    int ret;
	ret = 0;
	if([data length] >= 6)
	{
		NSRange r;
		uint8_t buf[10];
		r.location = 0;
		r.length = 10;
		ret = r.length;
		[data getBytes:buf length:10]; // Extract the complete Packet
		// process data received from the accessory
		switch(buf[0])
		{
			case 1: // Accessory Ready
				@synchronized (self)
            {
                /*AccStatus = buf[1];
                AccMajor = buf[2];
                AccMinor = buf[3];
                AccRev = buf[4];
                BoardID = buf[5];
                */
                ret=6;
            }
				break;
            case 4: // ReturnAccessorySwitches
				@synchronized (self)
            {
                if(((buf[0]+buf[1]+buf[2]+buf[3]+buf[4]+buf[5]+buf[6]+buf[7])&0XFF)==buf[8])
                {

                    _inputPacket.buttonA = buf[1] & MASK_BUTT_A;
                    _inputPacket.buttonB = buf[1] & MASK_BUTT_B;
                    _inputPacket.buttonX = buf[1] & MASK_BUTT_X;
                    _inputPacket.buttonY = buf[1] & MASK_BUTT_Y;
                    _inputPacket.buttonLeftStick = buf[1] & MASK_BUTT_L_STICK;
                    _inputPacket.buttonRightStick = buf[1] & MASK_BUTT_R_STICK;
                    _inputPacket.buttonStart = buf[1] & MASK_BUTT_START;
                    
                    _inputPacket.leftTrigger = buf[2] & MASK_TRIG_L;
                    _inputPacket.rightTrigger = buf[2] & MASK_TRIG_R;
                    _inputPacket.buttonL = buf[2] & MASK_BUTT_L;
                    _inputPacket.buttonR = buf[2] & MASK_BUTT_R;
                    _inputPacket.buttonU = buf[2] & MASK_BUTT_U;
                    _inputPacket.buttonD = buf[2] & MASK_BUTT_D;
                    
                    _inputPacket.leftStick[0] = buf[3];
                    _inputPacket.leftStick[1] = buf[4];
                    _inputPacket.rightStick[0] = buf[5];
                    _inputPacket.rightStick[1] = buf[6];
                    
                    
                    if([self delegate]) {
                        [[self delegate] duoAccessory:self didHaveInput:_inputPacket];
                    }
                }
                
                ret=9;
            }
				break;
			default: // unknown command
				break;
		}
	}
	return ret;
}

- (void)stream:(NSStream *)stream handleEvent:(NSStreamEvent)eventCode {	
    if(stream != _session.inputStream)
        return;
    
    uint8_t buf[20];
    unsigned int len = 0;

    switch (eventCode)
    {
        case NSStreamEventHasBytesAvailable: {           
            while((len = [(NSInputStream *)stream read:buf maxLength:sizeof(buf)]))
            {
				if(rxData == nil) rxData = [[NSMutableData data] retain];
				[rxData appendBytes:buf length:len];
			}            
			const char preambleBytes[] = {0x5A,0xA5};
			NSRange r = {0,sizeof(preambleBytes)};
			NSData	*pd = [NSData dataWithBytes:preambleBytes length:2];
            
			r.length = 2;
            
            while ((r.length + r.location) < [rxData length])
			{
                if([rxData length]<11)
                    break;
                
                r.length=2;
                if([[rxData subdataWithRange:r] isEqualToData:pd])
				{
					if( (r.location + r.length) > [rxData length])
						break;
                    
					r.location += 2;
                    
					r.length = [rxData length] - r.location;
                    
					len = [self readData:[rxData subdataWithRange:r]];
                    
					r.location += len;
                    
                    if(r.location<[rxData length])
                        r.length=[rxData length] - r.location;
                    else
                        r.length=0;
                    
                    [rxData setData:[rxData subdataWithRange:r]];
                    r.length=0;
                    r.location=0;
				}
				else
				{
					r.location ++;
				}
			}
            
			
        }    break;
        case NSStreamEventHasSpaceAvailable:{}
            // Send the next queued command.
            break;
        case NSStreamEventErrorOccurred:{}
            //NSLog(@"Stream error Occured");
            break;
        default:
            break;
    }
}

@end

@interface DuoThreadedAccessoryV2 ()
/*{
    NSRunLoop* _loop;
}*/
@end

@implementation DuoThreadedAccessoryV2

- (id) initWithProtocol:(NSString*)protocol
{
    self = [self initWithProtocol:protocol andRunLoop:[NSRunLoop mainRunLoop]];
    return self;
}

- (id) initWithProtocol:(NSString *)protocol andRunLoop:(NSRunLoop*)loop
{
    if(self = [super initWithProtocol:protocol])
    {
        _loop = loop;
    }
    
    return self;
}

- (void) beginListening
{
    [self initializePacket];
    
    [_session.inputStream setDelegate:self];
    [_session.inputStream scheduleInRunLoop:_loop
                                    forMode:NSDefaultRunLoopMode];
    [_session.inputStream open];
    
    [_session.outputStream setDelegate:self];
    [_session.outputStream scheduleInRunLoop:_loop
                                     forMode:NSDefaultRunLoopMode];
    [_session.outputStream open];
    
}

@end
#endif
